typedef struct {
	double x;
	double y;
	double z;
} edge_pixel_t, point_3d_t, plane_coeff;

typedef struct {
	point_3d_t centroid;
	point_3d_t meanCentroid;
	int votes;
} centroid_list;

typedef struct {
	edge_pixel_t point;
	edge_pixel_t centroid;
	int centroidIndex;
} point_list;

typedef struct {
	edge_pixel_t point;
	double distance;
} pixel_distance;

typedef struct
{
	int a;
	int b;
	int c;
	int alpha;
	int beta;
	int theta;
	int    centroid;
	int	   votes;
} AxisOrientation_t;

				edge_pixel_t*   	h_edge_pxls     	= NULL;
				int            		h_edge_pxl_cnt 		= 0;
				point_list* 		h_voters			= NULL;
				int 		  		h_numVoters 		= 0;
				centroid_list* 		h_possCentroids 	= NULL;							
__device__		edge_pixel_t*		d_edge_pxls			= NULL;
__device__		centroid_list* 		d_possCentroids 	= NULL;
__device__		point_list* 		d_voters			= NULL;
__device__ 		int					d_edge_pxl_cnt		= 0;
__device__		int					centroidCount		= 0;
				centroid_list*		h_nearest			= NULL;
__device__		centroid_list*		d_nearest			= NULL;
__device__		int*				d_nearest_voters	= NULL;
__device__		centroid_list*		d_finalCentroids	= NULL;
				centroid_list*		h_finalCentroids	= NULL;
__device__		point_list*			d_finalVoters		= NULL;
				point_list*			h_finalVoters		= NULL;
__device__		int 				d_centroidLocation 	= 0;
				int 				h_centroidLocation 	= 0;
__device__		int					voteLocation		= 0;
__device__		AxisOrientation_t*	d_axisOri			= NULL;
				AxisOrientation_t*	h_axisOri			= NULL;

__device__ 		bool coPlanar(edge_pixel_t*);
__device__ 		double calcDistance(edge_pixel_t, edge_pixel_t);
__device__ 		void insertionSort(pixel_distance* ,int);
__device__ 		void gauss(double (*A)[DATA_POINTS], uint rows, uint cols, double*);
__device__ 		void findTangentPlanes(plane_coeff*, edge_pixel_t*);
__device__ 		void fitTangentPlane(plane_coeff &plane, edge_pixel_t);
__device__ 		void findNeighborhood(edge_pixel_t, pixel_distance *);
__device__ 		void findIntersection(plane_coeff *, edge_pixel_t *, point_3d_t [][2]);
__device__ 		void findIntersectionPoint(plane_coeff plane1, plane_coeff plane2, point_3d_t &interPoint1, point_3d_t &interPoint2);
__device__ 		void hessianNormalForm(plane_coeff plane, plane_coeff & hessian, double & P);
__device__ 		void calcX0(plane_coeff hess1, plane_coeff hess2, double P1, double P2, point_3d_t & X0);
__device__ 		void directionFromNullSpace(plane_coeff hess1, plane_coeff hess2, point_3d_t & dir);
__device__ 		void findCentroid(plane_coeff * planes, point_3d_t * pxls, point_3d_t [][2]);
__device__ 		void findMidPoint(point_3d_t point1, point_3d_t point2, point_3d_t & midPoint);
__device__ 		void getLineMidpointPlanes(point_3d_t midPoint, plane_coeff & plane, int i, point_3d_t intersectionLines[][2]);
__device__		bool in_range(int, int, int);

//this function will allocate and transfer all necessary data structures
//and data from the host to the device


__host__ void setup_Centroid_Solver() 
{
	cudaError_t err;
	
	h_nearest = new centroid_list[MAX_CENTROIDS * EVIDENCE_TRSH];//(centroid_list*)malloc(sizeof(centroid_list) * MAX_CENTROIDS * EVIDENCE_TRSH);
	memset(h_nearest, 0,  sizeof(centroid_list) * MAX_CENTROIDS * EVIDENCE_TRSH);

	printf("setting up centroid solver...\n");
	err = cudaMalloc((void **)&d_edge_pxls, sizeof(edge_pixel_t) * h_edge_pxl_cnt);
	
	if (err != cudaSuccess)
		printf("%s",cudaGetErrorString(err));
		
	err = cudaMemcpy(d_edge_pxls, h_edge_pxls, sizeof(edge_pixel_t) * h_edge_pxl_cnt, cudaMemcpyHostToDevice);
	
	//~ memset(h_edge_pxls, 0, sizeof(edge_pixel_t) * h_edge_pxl_cnt);
	//~ 
	//~ for(int i = 0; i < h_edge_pxl_cnt; i++)
		//~ //printf("element %d: x: %f, y: %f, z: %f\n", i, h_edge_pxls[i].x, h_edge_pxls[i].y, h_edge_pxls[i].z);
	
	if (err != cudaSuccess)
		printf("%s",cudaGetErrorString(err));
		
	//~ cudaMemcpy(h_edge_pxls, d_edge_pxls, sizeof(edge_pixel_t) * h_edge_pxl_cnt, cudaMemcpyDeviceToHost);
//~ 
	//~ for(int i = 0; i < h_edge_pxl_cnt; i++)
		//~ //printf("element %d: x: %f, y: %f, z: %f\n", i, h_edge_pxls[i].x, h_edge_pxls[i].y, h_edge_pxls[i].z);
		
	err = cudaMalloc((void **)&d_voters, sizeof(point_list) * DATA_POINTS * EVIDENCE_TRSH);
	
	if (err != cudaSuccess)
		printf("%s",cudaGetErrorString(err));
		
	err = cudaMemset(d_voters, 0, sizeof(point_list) * DATA_POINTS * EVIDENCE_TRSH);
	
	if (err != cudaSuccess)
		printf("%s",cudaGetErrorString(err));
	
	printf("allocating d_nearest....\n");
	err = cudaMalloc((void **)&d_nearest, sizeof(centroid_list) * MAX_CENTROIDS * EVIDENCE_TRSH);
	
	printf("memset d_nearest....\n");
	cudaMemset(d_nearest, 0, sizeof(centroid_list) * MAX_CENTROIDS * EVIDENCE_TRSH);

	h_finalCentroids = (centroid_list*)malloc(sizeof(centroid_list) * MAX_CENTROIDS);
	memset(h_finalCentroids, 0, sizeof(centroid_list) * MAX_CENTROIDS);

	cudaMalloc((void **)&d_finalCentroids, sizeof(centroid_list) * MAX_CENTROIDS);
	cudaMemset(d_finalCentroids, 0, sizeof(centroid_list) * MAX_CENTROIDS);

	h_finalCentroids = (centroid_list*)malloc(sizeof(centroid_list) * MAX_CENTROIDS);

	cudaMalloc((void **)&d_finalVoters, sizeof(point_list) * DATA_POINTS * EVIDENCE_TRSH);
	cudaMemset(d_finalVoters, -1,  sizeof(point_list) * DATA_POINTS * EVIDENCE_TRSH);

	h_voters = (point_list*)malloc(sizeof(point_list) * DATA_POINTS * EVIDENCE_TRSH);
	h_finalVoters = (point_list*)malloc(sizeof(point_list) * DATA_POINTS * EVIDENCE_TRSH);

	//cudaMalloc((void**)d_centroidLocation, sizeof(int));
	//cudaMemset(d_centroidLocation, 0, sizeof(int));

	cudaMalloc((void**)&d_nearest_voters, sizeof(int) * MAX_CENTROIDS * 10);
	cudaMemset(d_nearest_voters, -1, sizeof(int) * MAX_CENTROIDS * 10);

	printf("solver finished...!\n");
	
}

__host__ void setup_accum() 
{
	// allocate the accumulator for the ellipse center
	cudaError_t err;
	
	h_possCentroids = new centroid_list[EVIDENCE_TRSH];
	memset(h_possCentroids,0,sizeof(centroid_list) * EVIDENCE_TRSH);
	
	//printf("accum setup called...\n");
	err = cudaMalloc((void **)&d_possCentroids, sizeof(centroid_list) * EVIDENCE_TRSH);
	cudaMemset(d_possCentroids, 0, sizeof(centroid_list) * EVIDENCE_TRSH);
	
	if (err != cudaSuccess)
		//printf("%s",cudaGetErrorString(err));
		
	err = cudaMemcpy(d_possCentroids, h_possCentroids, sizeof(centroid_list) * EVIDENCE_TRSH, cudaMemcpyHostToDevice);

	cudaMalloc((void**)&d_axisOri, sizeof(AxisOrientation_t) * AO_HIGH);
	cudaMemset(d_axisOri, 0,  sizeof(AxisOrientation_t) * AO_HIGH);

	h_axisOri = (AxisOrientation_t*)malloc( sizeof(AxisOrientation_t) * AO_HIGH);
	memset(h_axisOri, 0,  sizeof(AxisOrientation_t) * AO_HIGH);
		
	//printf("After setup accum.\n");
}

__global__ void gather_evidence(edge_pixel_t* d_edge_pixels,
		uint d_edge_pixels_cnt,
		uint* d_rnds,
		point_list* d_Voters,
		centroid_list* d_possibleCentroids
	)
{
	int 		evidence = 0, 
				next_rnd = 0,
				rnd = 0;
	
	d_edge_pxls 	= d_edge_pixels;
	d_edge_pxl_cnt 	= d_edge_pixels_cnt;
	d_possCentroids	= d_possibleCentroids;
	d_voters		= d_Voters;
	// allocate arrays in shared memory
	edge_pixel_t	pxls[DATA_POINTS];
	plane_coeff 	planes[4];
	point_3d_t 		intersectionLines[6][2];

	int index = (blockIdx.x * blockDim.x + threadIdx.x); 
	next_rnd = (blockIdx.x * blockDim.x + threadIdx.x) * DATA_POINTS;
	

	////printf("thread #%d\n", blockIdx.x * blockDim.x + threadIdx.x);
	
	// randomly select DATA_POINTS # of edge pixels
	do
	{

		////printf("thread #%d, next_rnd:%d\n", blockIdx.x * blockDim.x + threadIdx.x, next_rnd);
		
 		for (int i=0; i < DATA_POINTS; i++)
		{
			//((int)d_rnds[next_rnd++] % 64800); // (double)0xFFFFFFFF) * (d_edge_pixels_cnt);
			rnd = (d_rnds[next_rnd++]) % d_edge_pxl_cnt;// / (float)0xFFFFFFFF) * (d_edge_pixels_cnt);
			//printf("random number: %d.......next_rnd:%d\n", rnd, next_rnd);
			//rnd = d_rnds[next_rnd++]/((double)0xFFFFFFFF) * (d_edge_pixels_cnt));
			//printf("rnd #%d: %d, value: x:%f, y:%f, z:%f\n", next_rnd, d_rnds[next_rnd], d_edge_pxls[rnd].x,d_edge_pxls[rnd].y,d_edge_pxls[rnd].z  );
			pxls[i] = d_edge_pxls[rnd];
			//printf("got a new edge pixel ....\n");
		}
		
		
		
	}while(coPlanar(pxls));
		
//	//printf("thread: %d\n", (next_rnd-6)/6);
//	
//	for (int i=0; i < DATA_POINTS; i++)
	{
//		printf("thread #: %d...........x: %f,   y: %f,   z: %f\n",index, pxls[i].x, pxls[i].y, pxls[i].z);
	}
	
	findTangentPlanes(planes, pxls);
	

	findIntersection(planes, pxls, intersectionLines);


	findCentroid(planes, pxls, intersectionLines);

	
	//__syncthreads();
	
	//processAccum();
	
	//__syncthreads();
	
	//mergeAccum();


	//~ 
	//~ //printf("Iterations: %d\n", evidence);
	//~ //printf("Centroids\n");
	//~ for(int temp = 0; temp < centroidCount; temp++)
	//~ {
		//~ if(h_possCentroids[temp].votes >= CENTROID_TRSH_LOW)
		//~ {
			//~ //printf("%d: \t %f, %f, %f \t Votes: %d \n", temp, h_possCentroids[temp].centroid.x, h_possCentroids[temp].centroid.y, h_possCentroids[temp].centroid.z, h_possCentroids[temp].votes);
			//~ //printf("\t %f, %f, %f \n\n", h_possCentroids[temp].meanCentroid.x, h_possCentroids[temp].meanCentroid.y, h_possCentroids[temp].meanCentroid.z);
		//~ }
	//~ }
}

__device__ bool coPlanar(edge_pixel_t* pixels)
{
	edge_pixel_t pt31, pt21, pt43, crossProd;
	
	pt31.x = pixels[2].x - pixels[0].x;
	pt31.y = pixels[2].y - pixels[0].y;
	pt31.z = pixels[2].z - pixels[0].z;
	
	if(pt31.x == 0.0 && pt31.y == 0.0 && pt31.z == 0.0)
		return true;
	
	pt21.x = pixels[1].x - pixels[0].x;
	pt21.y = pixels[1].y - pixels[0].y;
	pt21.z = pixels[1].z - pixels[0].z;
	
	pt43.x = pixels[3].x - pixels[2].x;
	pt43.y = pixels[3].y - pixels[2].y;
	pt43.z = pixels[3].z - pixels[2].z;
	
	crossProd.x = pt21.y*pt43.z - pt43.y*pt21.z;
	crossProd.y = pt21.z*pt43.x - pt43.z*pt21.x;
	crossProd.z = pt21.x*pt43.y - pt43.x*pt21.y;
	
	if((pt31.x*crossProd.x + pt31.y*crossProd.y + pt31.z*crossProd.z) == 0.0)
		return true;
		
	return false;
}

__device__ void findTangentPlanes(plane_coeff * planes, edge_pixel_t* pxls)
{
	////printf("seg fault not here\n");
	if((blockIdx.x * blockDim.x + threadIdx.x) == 0)
		{
			////printf("========================================================================================================\n\n");
		}
	for(int i = 0; i < DATA_POINTS; i++)
	{
		fitTangentPlane(planes[i], pxls[i]);
	}
	/*
	printf("*************Tangent Planes*************\n");
 	//printf("thread id: %d\n", (blockIdx.x * blockDim.x + threadIdx.x));
	for(int i = 0; i < DATA_POINTS; i++)
	{
		printf("....... x: %f,  y: %f,  z: %f\n", planes[i].x,planes[i].y, planes[i].z);
	} 
	printf("*************End Tangent Planes*************\n");
	*/
}

__device__ void fitTangentPlane(plane_coeff &plane, edge_pixel_t pxl)
{
	int i,j;
	double sum_xx = 0.0, sum_yy = 0.0, sum_xy = 0.0,
		   sum_x = 0.0, sum_y = 0.0, sum_xz = 0.0, sum_yz = 0.0, sum_z = 0.0;
	
	// allocate
	double aug[3][4] = {{0.0},{0.0}};
	double results[3] = {0.0};
	pixel_distance closest[7];
	
		

	
	findNeighborhood(pxl, closest);
	
	for(int i = 0; i < 6; i++)
	{
		sum_xx += closest[i].point.x * closest[i].point.x;
		sum_yy += closest[i].point.y * closest[i].point.y;
		sum_xy += closest[i].point.x * closest[i].point.y;
		sum_xz += closest[i].point.x * closest[i].point.z;
		sum_yz += closest[i].point.y * closest[i].point.z;
		sum_x  += closest[i].point.x;
		sum_y  += closest[i].point.y;
		sum_z  += closest[i].point.z;
	}
	
	
//aug: .... sum_xx: 9432.390055     sum_xy: -11218.026768     sum_x :237.890021     sum_xz: 15318.137258
//aug: .... sum_xy: -11218.026768     sum_yy: 13342.289938     sum_y :-282.937686     sum_yz: -18217.392893
//aug: .... sum_x : 237.890021     sum_y : -282.937686           :6.000000     sum_z : 386.318434

	
/* 	aug[0][0] = 9432.390055;
	aug[0][1] = -11218.026768;
	aug[1][0] = -11218.026768;
	aug[0][2] = 237.890021;
	aug[2][0] = 237.890021;
	aug[1][1] = 13342.289938;
	aug[1][2] = -282.937686;
	aug[2][1] = -282.937686;
	aug[2][2] = 6;
	aug[0][3] = 15318.137258;
	aug[1][3] = -18217.392893;
	aug[2][3] = 386.318434; */
	
	aug[0][0] = sum_xx;
	aug[0][1] = sum_xy;
	aug[1][0] = sum_xy;
	aug[0][2] = sum_x;
	aug[2][0] = sum_x;
	aug[1][1] = sum_yy;
	aug[1][2] = sum_y;
	aug[2][1] = sum_y;
	aug[2][2] = 6;
	aug[0][3] = sum_xz;
	aug[1][3] = sum_yz;
	aug[2][3] = sum_z;
	
	/*
  	printf("before ============================================================\n");
	printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     sum_xz: %f\n", aug[0][0], aug[0][1], aug[0][2], aug[0][3]);
	printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     sum_yz: %f\n", aug[1][0], aug[1][1], aug[1][2], aug[1][3]);
	printf("aug: .... sum_x : %f     sum_y : %f           :%f     sum_z : %f\n", aug[2][0], aug[2][1], aug[2][2], aug[2][3]);
	printf("before ============================================================\n"); 
	

	// run a gaussian solver on the system of equations
	printf("==========================================\n");
	*/
	//matrix_print("Augmented (pre-gauss)",&aug);
	gauss(aug, 3, 4, results);
	//~ matrix_print("Augmented (post-gauss)",&aug);
	/*
	printf("==========================================\n");
	
 	printf("after ============================================================\n");
	printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     A: %f\n", aug[0][0], aug[0][1], aug[0][2], aug[0][3]);
	printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     B: %f\n", aug[1][0], aug[1][1], aug[1][2], aug[1][3]);
	printf("aug: .... sum_x : %f     sum_y : %f           :%f     C: %f\n", aug[2][0], aug[2][1], aug[2][2], aug[2][3]);
	printf("after ============================================================\n"); 
	*/
	// extract the results
	plane.x = results[0];
	plane.y = results[1];
	plane.z = results[2];
	
	/*
	printf("A: %f\n", plane.x);
	printf("B: %f\n", plane.y);
	printf("C: %f\n\n", plane.z);
	*/
	////printf("divide: %f\n\n\n\n\n\n", 0.00000694 / -0.00000984);

	// cleanup
	//delete [] results;

}

__device__ void findNeighborhood(edge_pixel_t currPoint, pixel_distance *closest)
{
	int i = 0;
	
	//~ //printf("findNeighborhood called\n");
	//~ //printf("thread id: %d .... x: %f, y: %f, z: %f\n", (blockIdx.x * blockDim.x + threadIdx.x), currPoint.x, currPoint.y, currPoint.z);

	for(i = 0; i < 6; i++)
	{
		closest[i].point = d_edge_pxls[i];
		closest[i].distance = calcDistance(currPoint, closest[i].point);
	}
	
	//~ //printf("closest neighbors before: \n");
	//for(int i = 0; i < 7; i++)
	////printf("thread id: %d .... x: %f, y: %f, z: %f, distance: %f\n", (blockIdx.x * blockDim.x + threadIdx.x), closest[i].point.x, closest[i].point.y, closest[i].point.z, closest[i].distance);
	
	while(i < d_edge_pxl_cnt)
	{
		
		closest[6].point = d_edge_pxls[i];
		closest[6].distance = calcDistance(currPoint, closest[6].point);		
		insertionSort(closest, 7);
		i++;
	}
	
	////printf("closest neighbors after: \n");
	//for(int i = 0; i < 7; i++)
		////printf("thread id: %d .... x: %.8f, y: %.8f, z: %.8f, distance: %.8f\n", (blockIdx.x * blockDim.x + threadIdx.x), closest[i].point.x, closest[i].point.y, closest[i].point.z, closest[i].distance);
	
}

__device__ double calcDistance(edge_pixel_t p1, edge_pixel_t p2)
{
	return sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y) + (p1.z - p2.z)*(p1.z - p2.z));
}

__device__ void insertionSort(pixel_distance arr[],int length)
{
	int i, j;
	pixel_distance tmp;

	for(i = 1; i < length; i++)
	{
		j = i;
		while( j > 0 && arr[j-1].distance > arr[j].distance)
		{
			tmp = arr[j];
			arr[j] = arr[j-1];
			arr[j-1] = tmp;
			j--;
		}
	}
}

// implemented from psudocode at: http://en.wikipedia.org/wiki/Gaussian_elimination#Pseudocode
// gaussian elimination with partial pivoting
// results is ordered x1,x2,x3...xn
__device__ void gauss(double (*A)[DATA_POINTS], uint rows, uint cols, double* results) 
{
	int i,j,k,u,maxi;
	double var, t;
	
	////printf("TESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTEST\n");
	////printf("%.10f\n", -0.949636619 - (-3.26690635 * 0.29059448));
	////printf("%.10f\n", -0.94982068 - (-3.26720488 * 0.28972552));
	////printf("%.10f\n", sqrt((-0.059874 * 0.06987) / 0.258000068));
	////printf("Fast sqrt inverse%.10f\n", Q_rsqrt((-0.059874 * 0.06987) / 0.258000068));
	////printf("TESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTESTTEST\n");
	
	double aug[3][4] = {{0.0}, {0.0}};
	
	for(int i = 0; i < 3; i++)
		for(int j = 0; j < 4; j++)
			aug[i][j] = A[i][j];

	i = 0;
	j = 0;
	
	while (i < rows && j < cols) {
		// find the row with the maximum value
		maxi = i;
		for (k=i+1; k < rows; k++) {
			if (fabs(A[k][j]) > fabs(A[maxi][j]))
			{
				maxi = k;
				////printf("A[k][j] = %.8f ..... A[maxi][j] = %.8f\n\n", fabs(A[k][j]),fabs(A[maxi][j]));
			} 
		}
	
		/*
  		//printf("highest value found in for %d\n", maxi);
		printf("after first for loop ============================================================\n");
		printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     sum_xz: %f\n", A[0][0], A[0][1], A[0][2], A[0][3]);
		printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     sum_yz: %f\n", A[1][0], A[1][1], A[1][2], A[1][3]);
		printf("aug: .... sum_x : %f     sum_y : %f           :%f     sum_z : %f\n", A[2][0], A[2][1], A[2][2], A[2][3]);
		printf("after first for loop ============================================================\n");   
		*/

		if (fabs(A[maxi][j]) != 0 ) {
			// swap rows
			if (i != maxi) {
				for (u=0; u < cols; u++) {
					
					/*
					printf("before swap %d ============================================================\n", u);
					printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     sum_xz: %f\n", A[0][0], A[0][1], A[0][2], A[0][3]);
					printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     sum_yz: %f\n", A[1][0], A[1][1], A[1][2], A[1][3]);
					printf("aug: .... sum_x : %f     sum_y : %f           :%f     sum_z : %f\n", A[2][0], A[2][1], A[2][2], A[2][3]);
					printf("before swap ============================================================\n");  
					*/

					t = A[i][u];
					A[i][u] = A[maxi][u];
					A[maxi][u] = t;

					/*
 					printf("during swap %d ============================================================\n", u);
					printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     sum_xz: %f\n", A[0][0], A[0][1], A[0][2], A[0][3]);
					printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     sum_yz: %f\n", A[1][0], A[1][1], A[1][2], A[1][3]);
					printf("aug: .... sum_x : %f     sum_y : %f           :%f     sum_z : %f\n", A[2][0], A[2][1], A[2][2], A[2][3]);
					printf("during swap ============================================================\n\n\n");   
					*/
				}
			}
			


			// reduce pivot element to 1
			var = A[i][j];
			//var *= 10000;
			for (k=0; k < cols; k++)
			{
				/*
				printf("PIVOTING\n");
				printf("before pivot with %.8f ============================================================\n", var);
				printf("aug: .... sum_xx: %.8f         sum_xy: %.8f     sum_x :%.8f         sum_xz: %.8f\n", A[0][0], A[0][1], A[0][2], A[0][3]);
				printf("aug: .... sum_xy: %.8f         sum_yy: %.8f     sum_y :%.8f         sum_yz: %.8f\n", A[1][0], A[1][1], A[1][2], A[1][3]);
				printf("aug: .... sum_x : %.8f         sum_y : %.8f           :%.8f         sum_z : %.8f\n", A[2][0], A[2][1], A[2][2], A[2][3]);
				printf("before pivot ============================================================\n"); 
				*/

				//double temp = (A[i][k]*10000) / var;
				//////printf("value of temp: %f\n", temp);
				A[i][k] /= var;
				//A[i][k] = temp;
				/*
				printf("pivoted [%d, %d]\n", i, k);
				printf("after pivot ============================================================\n");
				printf("aug: .... sum_xx: %.8f         sum_xy: %.8f     sum_x :%.8f         sum_xz: %.8f\n", A[0][0], A[0][1], A[0][2], A[0][3]);
				printf("aug: .... sum_xy: %.8f         sum_yy: %.8f     sum_y :%.8f         sum_yz: %.8f\n", A[1][0], A[1][1], A[1][2], A[1][3]);
				printf("aug: .... sum_x : %.8f         sum_y : %.8f           :%.8f         sum_z : %.8f\n", A[2][0], A[2][1], A[2][2], A[2][3]);
				printf("dafter pivot ============================================================\n\n\n"); 
				*/
			}
				


			// remove the pivot element from all subsequent rows
			for (u=i+1; u < rows; u++) {
				var = A[u][j];
				/*
				printf("var: %f ..... [%d, %d] \n", var, u, j);
				printf("before remove pivot, row %d ============================================================\n", u);
				printf("aug: .... sum_xx: %.8f         sum_xy: %.8f     sum_x :%.8f         sum_xz: %.8f\n", A[0][0], A[0][1], A[0][2], A[0][3]);
				printf("aug: .... sum_xy: %.8f         sum_yy: %.8f     sum_y :%.8f         sum_yz: %.8f\n", A[1][0], A[1][1], A[1][2], A[1][3]);
				printf("aug: .... sum_x : %.8f         sum_y : %.8f           :%.8f         sum_z : %.8f\n", A[2][0], A[2][1], A[2][2], A[2][3]);
				printf("before remove pivot ============================================================\n\n\n"); 
				*/
				for (k=j; k < cols; k++)
				{
					////printf("equation: %.8f - (%.8f * %.8f)",A[u][k], A[i][k], var);
					A[u][k] -= A[i][k] * var;
					////printf("= %.8f\n\n", A[u][k]);
				}
				/*	
				printf("after remove pivot, row %d ============================================================\n", u);
				printf("aug: .... sum_xx: %.8f         sum_xy: %.8f     sum_x :%.8f         sum_xz: %.8f\n", A[0][0], A[0][1], A[0][2], A[0][3]);
				printf("aug: .... sum_xy: %.8f         sum_yy: %.8f     sum_y :%.8f         sum_yz: %.8f\n", A[1][0], A[1][1], A[1][2], A[1][3]);
				printf("aug: .... sum_x : %.8f         sum_y : %.8f           :%.8f         sum_z : %.8f\n", A[2][0], A[2][1], A[2][2], A[2][3]);
				printf("after remove pivot ============================================================\n\n\n"); 
				*/
			}

			i++;
		}

		j++;
	}

	// retrieve the results
	for (i=rows-1; i >= 0; i--) {
		var = A[i][cols-1];
		for (j=cols-2; j > i; j--) {
			var -= A[i][j] * results[j];
		}
		results[i] = var;
	}
}

__device__ void findIntersection(plane_coeff * planes, edge_pixel_t * pxls, point_3d_t intersectionLines[][2])
{	
	findIntersectionPoint(planes[0], planes[1], intersectionLines[0][0], intersectionLines[0][1]);
	findIntersectionPoint(planes[0], planes[2], intersectionLines[1][0], intersectionLines[1][1]);
	findIntersectionPoint(planes[0], planes[3], intersectionLines[2][0], intersectionLines[2][1]);
	findIntersectionPoint(planes[1], planes[2], intersectionLines[3][0], intersectionLines[3][1]);
	findIntersectionPoint(planes[1], planes[3], intersectionLines[4][0], intersectionLines[4][1]);
	findIntersectionPoint(planes[2], planes[3], intersectionLines[5][0], intersectionLines[5][1]);
}

__device__ void findIntersectionPoint(plane_coeff plane1, plane_coeff plane2, point_3d_t &interPoint1, point_3d_t &interPoint2)
{
	plane_coeff 	hessian1, hessian2;
	double 			P1, P2;
	point_3d_t 		X0, direction_X0;
	
	hessianNormalForm(plane1, hessian1, P1);
	hessianNormalForm(plane2, hessian2, P2);
	
	calcX0(hessian1, hessian2, P1, P2, X0);
	directionFromNullSpace(hessian1, hessian2, direction_X0);
	
	interPoint1.x = X0.x;
	interPoint1.y = X0.y;
	interPoint1.z = X0.z;
	
	
	interPoint2.x = direction_X0.x;
	interPoint2.y = direction_X0.y;
	interPoint2.z = direction_X0.z;
/*    
    printf("Tangent Plane 1 Coefficients:\n");
	printf("\tA: %f\n", plane1.x);
	printf("\tB: %f\n", plane1.y);
	printf("\tC: %f\n\n", plane1.z);  
//  //~ 
    printf("Hessian Plane 1 Coefficients:\n");
	printf("\tA: %f\n", hessian1.x);
	printf("\tB: %f\n", hessian1.y);
	printf("\tC: %f\n\n", hessian1.z);
//	 
	printf("Hessian Plane 1 P Value: %f\n\n", P1);
/////~ 
	printf("Tangent Plane 2 Coefficients:\n");
	printf("\tA: %f\n", plane2.x);
	printf("\tB: %f\n", plane2.y);
	printf("\tC: %f\n\n", plane2.z);
	 //~ 
    printf("Hessian Plane 2 Coefficients:\n");
	printf("\tA: %f\n", hessian2.x);
	printf("\tB: %f\n", hessian2.y);
	printf("\tC: %f\n\n", hessian2.z);
////	//~ 
	printf("Hessian Plane 2 P Value: %f\n\n", P2);
////	
//	//~ ////printf("Tangent Plane 3 Coefficients:\n");
//	//~ ////printf("\tA: %f\n", plane3.x);
//	//~ ////printf("\tB: %f\n", plane3.y);
//	//~ ////printf("\tC: %f\n\n", plane3.z);
////    
    printf("X0 vector:\n");
    printf("x: %f\n", X0.x);
    printf("y: %f\n", X0.y);
    printf("z: %f\n\n", X0.z);
////     
    printf("Direction vector:\n");
    printf("x: %f\n", direction_X0.x);
    printf("y: %f\n", direction_X0.y);
    printf("z: %f\n\n", direction_X0.z);
////    
    printf("Intersection Point 1: %f, %f, %f\n\n", interPoint1.x, interPoint1.y, interPoint1.z);
    printf("Intersection Point 2: %f, %f, %f\n\n", interPoint2.x, interPoint2.y, interPoint2.z);
    printf("----------------------------------------------------------------------\n"); 
   
    //~ delete [] results;
	*/
}


__device__ void hessianNormalForm(plane_coeff plane, plane_coeff & hessian, double & P)
{
	////printf("HESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIAN\n");
	////printf("plane.x: %.10f\n", plane.x);
	////printf("plane.y: %.10f\n", plane.y);
	
	////printf("hessian.x divisor before sqrt(): %.10f\n", plane.x*plane.x + plane.y*plane.y + 1);
	////printf("hessian.x divisor after  sqrt(): %.10f\n", sqrt(plane.x*plane.x + plane.y*plane.y + 1));
	////printf("hessian.x divisor after  fastsqrt(): %.10f\n", fastsqrt(plane.x*plane.x + plane.y*plane.y + 1));

	////printf("P: %.10f\n", P);
		////printf("HESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIAN\n");
		
	hessian.x = plane.x / (sqrt(plane.x*plane.x + plane.y*plane.y + 1));
	hessian.y = plane.y / (sqrt(plane.x*plane.x + plane.y*plane.y + 1));
	hessian.z = -1 / (sqrt(plane.x*plane.x + plane.y*plane.y + 1));
	P = plane.z / (sqrt(plane.x*plane.x + plane.y*plane.y + 1));
	/*
	printf("HESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIAN\n");
	printf("hessian.x: %.10f\n", hessian.x);
	printf("hessian.y: %.10f\n", hessian.y);
	printf("hessian.z: %.10f\n", hessian.z);
	printf("P: %.10f\n", P);
	printf("HESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIANHESSIAN\n");
	*/
}

__device__ void calcX0(plane_coeff hess1, plane_coeff hess2, double P1, double P2, point_3d_t & X0)
{
	//matrix_allocate(&aug, 3, 4);
	
	double aug[3][4];
	double results[3];
	
	//double * results;
	//results = new double[3];
	//memset(results,0,sizeof(double) * 3);
	
	aug[0][0] = hess1.x;
	aug[0][1] = hess1.y;
	aug[0][2] = hess1.z;
	aug[0][3] = -P1;
	aug[1][0] = hess2.x;
	aug[1][1] = hess2.y;
	aug[1][2] = hess2.z;
	aug[1][3] = -P2;
	aug[2][0] = 0;
	aug[2][1] = 0;
	aug[2][2] = 1;
	aug[2][3] = hess1.z;
	
	/*	
	printf("before X0 ========================= X0 ================= X0 =================\n\n");
	printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     sum_xz: %f\n", aug[0][0], aug[0][1], aug[0][2], aug[0][3]);
	printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     sum_yz: %f\n", aug[1][0], aug[1][1], aug[1][2], aug[1][3]);
	printf("aug: .... sum_x : %f     sum_y : %f           :%f     sum_z : %f\n", aug[2][0], aug[2][1], aug[2][2], aug[2][3]);
	printf("before X0 ========================= X0 ================= X0 =================\n\n");
	*/
	//~ ////printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug);
	gauss(aug,3, 4, results);
	//~ matrix_print("Augmented (post-gauss)",&aug);
	//~ ////printf("==========================================\n");
	/*
	printf("after X0 ========================= X0 ================= X0 =================\n\n");
	printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     sum_xz: %f\n", aug[0][0], aug[0][1], aug[0][2], aug[0][3]);
	printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     sum_yz: %f\n", aug[1][0], aug[1][1], aug[1][2], aug[1][3]);
	printf("aug: .... sum_x : %f     sum_y : %f           :%f     sum_z : %f\n", aug[2][0], aug[2][1], aug[2][2], aug[2][3]);
	printf("after X0 ========================= X0 ================= X0 =================\n\n");
	*/
	//~ matrix_allocate(&aug2, 3, 4);
	//~ 
	//~ for (int i=0; i < aug.Y_SIZE; i++) {
		//~ for(int j=0; j < aug.X_SIZE; j++)
			//~ aug2.cells[i][j] = aug.cells[i][j];
	//~ }
	//~ 
	//~ aug2.cells[2][0] = 0;
	//~ aug2.cells[2][1] = 0;
	//~ aug2.cells[2][2] = 1;
	//~ aug2.cells[2][3] = hess1.z;
	//~ 
	//~ results = new double[3];
	//~ memset(results,0,sizeof(double) * 3);
	
	//~ ////printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug2);
	//~ gauss(&aug2,results);
	//~ matrix_print("Augmented (post-gauss)",&aug2);
	//~ ////printf("==========================================\n");
	
	X0.x = results[0];
	X0.y = results[1];
	X0.z = results[2];
	
	/*
	printf("X0.x: %f\n", X0.x);
	printf("X0.y: %f\n", X0.y);
	printf("X0.z: %f\n", X0.z);
	*/
	
	//delete [] results;
}

__device__ void directionFromNullSpace(plane_coeff hess1, plane_coeff hess2, point_3d_t & dir)
{
	// matrix_allocate(&aug, 3, 4);
	
	// double * results;
	// results = new double[3];
	// memset(results,0,sizeof(double) * 3);
	
	double aug[3][4];
	double results[3];
	
	aug[0][0] = hess1.x;
	aug[0][1] = hess1.y;
	aug[0][2] = hess1.z;
	aug[0][3] = 0;
	aug[1][0] = hess2.x;
	aug[1][1] = hess2.y;
	aug[1][2] = hess2.z;
	aug[1][3] = 0;
	aug[2][0] = 0;
	aug[2][1] = 0;
	aug[2][2] = 1;
	aug[2][3] = hess1.z;
	
	//~ matrix_allocate(&aug2, 3, 4);
	//~ 
	//~ for (int i=0; i < aug.Y_SIZE; i++) {
		//~ for(int j=0; j < aug.X_SIZE; j++)
			//~ aug2.cells[i][j] = aug.cells[i][j];
	//~ }
	//~ 
	//~ aug2.cells[2][0] = 0;
	//~ aug2.cells[2][1] = 0;
	//~ aug2.cells[2][2] = 1;
	//~ aug2.cells[2][3] = hess1.z;
	//~ 
	//~ results = new double[3];
	//~ memset(results,0,sizeof(double) * 3);
	
	//~ ////printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug2);
	//~ gauss(&aug2,results);
	//~ matrix_print("Augmented (post-gauss)",&aug2);
	//~ ////printf("==========================================\n");
	
	gauss(aug,3, 4, results);
	
	dir.x = results[0];
	dir.y = results[1];
	dir.z = results[2];
	
	/*
	printf("dir.x: %f\n", dir.x);
	printf("dir.y: %f\n", dir.y);
	printf("dir.z: %f\n", dir.z);
	*/
//	
	//delete [] results;
}


__device__ void findCentroid(plane_coeff * planes, point_3d_t * pxls, point_3d_t intersectionLines[][2])
{
	//point_3d_t * midPoints = new point_3d_t[4];
	//plane_coeff * lineMidpointPlanes = new plane_coeff[4];
	//point_3d_t * c_vecs = new point_3d_t[DATA_POINTS];
	//point_3d_t * pointsOnInterLines = new point_3d_t[6];
	point_3d_t 	tempPt1, tempPt2, P_L1, P_L2, centroidPt, p1, p2;
	double 		d, u;
	int			count = 0, index = 0;
	
	point_3d_t 	midPoints[4];
	plane_coeff 	lineMidpointPlanes[4];
	
	findMidPoint(pxls[0], pxls[1], midPoints[0]);
	findMidPoint(pxls[0], pxls[2], midPoints[1]);
	findMidPoint(pxls[0], pxls[3], midPoints[2]);
	findMidPoint(pxls[1], pxls[2], midPoints[3]);
	//~ findMidPoint(h_edge_pxls[pxls[1]], h_edge_pxls[pxls[3]], midPoints[4]);
	//~ findMidPoint(h_edge_pxls[pxls[2]], h_edge_pxls[pxls[3]], midPoints[2]);
	
	/*
	printf("Midpoint 12:\n");
    printf("x: %f\n", midPoints[0].x);
    printf("y: %f\n", midPoints[0].y);
    printf("z: %f\n\n", midPoints[0].z);
//    //~ 
    printf("Point 1:\n");
    printf("x: %f\n", pxls[0].x);
    printf("y: %f\n", pxls[0].y);
    printf("z: %f\n\n", pxls[0].z);
    printf("Midpoint 13:\n");
    printf("x: %f\n", midPoints[1].x);
    printf("y: %f\n", midPoints[1].y);
    printf("z: %f\n\n", midPoints[1].z);
    printf("Point 3:\n");
    printf("x: %f\n", pxls[2].x);
    printf("y: %f\n", pxls[2].y);
    printf("z: %f\n\n", pxls[2].z);
//    //~ 
    printf("Midpoint 23:\n");
    printf("x: %f\n", midPoints[2].x);
    printf("y: %f\n", midPoints[2].y);
    printf("z: %f\n\n", midPoints[2].z);
	*/

	getLineMidpointPlanes(midPoints[0], lineMidpointPlanes[0], 0, intersectionLines);
	getLineMidpointPlanes(midPoints[1], lineMidpointPlanes[1], 1, intersectionLines);
	getLineMidpointPlanes(midPoints[2], lineMidpointPlanes[2], 2, intersectionLines);
	getLineMidpointPlanes(midPoints[3], lineMidpointPlanes[3], 3, intersectionLines);
	
	/*
	printf("Plane 12 Coefficients:\n");
	printf("\tA: %f\n", lineMidpointPlanes[0].x);
	printf("\tB: %f\n", lineMidpointPlanes[0].y);
	printf("\tC: %f\n\n", lineMidpointPlanes[0].z);
	 
	printf("Plane 13 Coefficients:\n");
	printf("\tA: %f\n", lineMidpointPlanes[1].x);
	printf("\tB: %f\n", lineMidpointPlanes[1].y);
	printf("\tC: %f\n\n", lineMidpointPlanes[1].z);
	 
	printf("Plane 14 Coefficients:\n");
	printf("\tA: %f\n", lineMidpointPlanes[2].x);
	printf("\tB: %f\n", lineMidpointPlanes[2].y);
	printf("\tC: %f\n\n", lineMidpointPlanes[2].z);
	//~ 
	printf("Plane 23 Coefficients:\n");
	printf("\tA: %f\n", lineMidpointPlanes[3].x);
	printf("\tB: %f\n", lineMidpointPlanes[3].y);
	printf("\tC: %f\n\n", lineMidpointPlanes[3].z);
	*/

	findIntersectionPoint(lineMidpointPlanes[0], lineMidpointPlanes[3], tempPt1, tempPt2);
	
	/*
	printf("Point 1:\n");
    printf("x: %f\n", 	tempPt1.x);
    printf("y: %f\n", 	tempPt1.y);
    printf("z: %f\n\n", tempPt1.z);
    //~ 
	printf("Point 2:\n");
    printf("x: %f\n", 	tempPt2.x);
    printf("y: %f\n", 	tempPt2.y);
    printf("z: %f\n\n", tempPt2.z);
    */

    tempPt2.x = tempPt1.x + 100.0*tempPt2.x;
    tempPt2.y = tempPt1.y + 100.0*tempPt2.y;
    tempPt2.z = tempPt1.z + 100.0*tempPt2.z;
    
	/*
	printf("Point 2...AGAIN!:\n");
    printf("x: %f\n", 	tempPt2.x);
    printf("y: %f\n", 	tempPt2.y);
    printf("z: %f\n\n", tempPt2.z);
    
    //~ d = 
    */

	u = ((lineMidpointPlanes[2].x*tempPt1.x)+(lineMidpointPlanes[2].y*tempPt1.y)-tempPt1.z+lineMidpointPlanes[2].z) /
		((lineMidpointPlanes[2].x*(tempPt1.x-tempPt2.x))+(lineMidpointPlanes[2].y*(tempPt1.y-tempPt2.y))-(tempPt1.z-tempPt2.z));
	
	//~ cout << u << endl;
	
	//~ centroidPt.x = 5*(1.0-d) + d*2;
	//~ centroidPt.y = 0*(1.0-d) + d*0;
	//~ centroidPt.z = 0*(1.0-d) + d*0;
	
	centroidPt.x = (tempPt1.x*(1.0-u) + u*tempPt2.x);
	centroidPt.y = (tempPt1.y*(1.0-u) + u*tempPt2.y);
	centroidPt.z = (tempPt1.z*(1.0-u) + u*tempPt2.z);
	
	/*
	printf("Centroid:\n");
    printf("x: %f\n", 	centroidPt.x);
    printf("y: %f\n", 	centroidPt.y);
    printf("z: %f\n\n", centroidPt.z);
    */
	//place vote in possCentroid array
	
	index = blockIdx.x * blockDim.x + threadIdx.x;
    d_possCentroids[index].centroid.x = centroidPt.x;
    d_possCentroids[index].centroid.y = centroidPt.y;
    d_possCentroids[index].centroid.z = centroidPt.z;
    d_possCentroids[index].meanCentroid.x = centroidPt.x;
    d_possCentroids[index].meanCentroid.y = centroidPt.y;
    d_possCentroids[index].meanCentroid.z = centroidPt.z;
    d_possCentroids[index].votes++;

    //place pixels that were used to vote for centroid in votes array
    d_voters[index * DATA_POINTS].point             	= pxls[0];
    d_voters[index * DATA_POINTS].centroidIndex     	= index;
	d_voters[index * DATA_POINTS].centroid				= centroidPt;
    d_voters[index * DATA_POINTS + 1].point 			= pxls[1];
    d_voters[index * DATA_POINTS + 1].centroidIndex     = index;
	d_voters[index * DATA_POINTS + 1].centroid			= centroidPt;
    d_voters[index * DATA_POINTS + 2].point 			= pxls[2];
    d_voters[index * DATA_POINTS + 2].centroidIndex     = index;
	d_voters[index * DATA_POINTS + 2].centroid			= centroidPt;
    d_voters[index * DATA_POINTS + 3].point 			= pxls[3];
    d_voters[index * DATA_POINTS + 3].centroidIndex     = index;
	d_voters[index * DATA_POINTS + 3].centroid			= centroidPt;
   

//	printf("i: %d, x: %.8f, y:%.8f, z:%.8f, vote: %d\n", index,  d_voters[index].point.x, d_voters[index].point.y, d_voters[index].point.z, d_voters[index].centroidIndex);
    
   // //printf("thread %d.....x: %f, y: %f, z:%f\n", index, d_possCentroids[index].centroid.x, d_possCentroids[index].centroid.y, d_possCentroids[index].centroid.z);
	
	////printf("thread: %d\n", index);
	////printf("thread: %d\n", index);
    
    //~ 
	//printf("Point 2:\n");
    //printf("x: %f\n", pxls[1].x);
    //printf("y: %f\n", pxls[1].y);
    //printf("z: %f\n\n", pxls[1].z);
    //~ 
	//printf("Midpoint:\n");
    //printf("x: %f\n", midPoints[0].x);
    //printf("y: %f\n", midPoints[0].y);
    //printf("z: %f\n\n", midPoints[0].z);
}


//~ void findMeanCentroid(accum_element_centroid accum_val, point_3d_t & meanCentroid)
//~ {
	//~ double sum_x = 0, sum_y = 0, sum_z = 0;
	
	//~ for(int i = 0; i < accum_val.votes; i++)
	//~ {
		//~ sum_x = accum_val.pts.cells[i][0].x;
		//~ sum_y = accum_val.pts.cells[i][0].y;
		//~ sum_z = accum_val.pts.cells[i][0].z;
	//~ }
	
	//~ meanCentroid.x = sum_x / (double)accum_val.votes;
	//~ meanCentroid.y = sum_y / (double)accum_val.votes;
	//~ meanCentroid.z = sum_z / (double)accum_val.votes;
//~ }


__device__ void findMidPoint(point_3d_t point1, point_3d_t point2, point_3d_t & midPoint)
{
	midPoint.x = (point2.x + point1.x) / 2.0;
	midPoint.y = (point2.y + point1.y) / 2.0;
	midPoint.z = (point2.z + point1.z) / 2.0;
}

__device__ void getLineMidpointPlanes(point_3d_t midPoint, plane_coeff & plane, int i, point_3d_t intersectionLines[][2])
{
	// This works...I think
	point_3d_t B, C, AB, AC, N;
	B.x = intersectionLines[i][0].x + 0.3 * intersectionLines[i][1].x;
	B.y = intersectionLines[i][0].y + 0.3 * intersectionLines[i][1].y;
	B.z = intersectionLines[i][0].z + 0.3 * intersectionLines[i][1].z;
	
	C.x = intersectionLines[i][0].x + 0.6 * intersectionLines[i][1].x;
	C.y = intersectionLines[i][0].y + 0.6 * intersectionLines[i][1].y;
	C.z = intersectionLines[i][0].z + 0.6 * intersectionLines[i][1].z;
	//~ 
	//~ AB.x = B.x - midPoint.x;
	//~ AB.y = B.y - midPoint.y;
	//~ AB.z = B.z - midPoint.z;
	//~ 
	//~ AC.x = C.x - midPoint.x;
	//~ AC.y = C.y - midPoint.y;
	//~ AC.z = C.z - midPoint.z;
	//~ 
	//~ crossProduct(AB, AC, plane);
	
	//~ //printf("Normal:\n");
    //~ //printf("x: %f\n", 	plane.x);
    //~ //printf("y: %f\n", 	plane.y);
    //~ //printf("z: %f\n\n", plane.z);
	
	if(2 == i)
	{
		//printf("Normal=======================================================================================================:\n");
		//printf("B.x %.8f\n", B.x);
		//printf("B.y %.8f\n", B.y);
		//printf("B.z %.8f\n", B.z);
		
		//printf("C.x %.8f\n", C.x);
		//printf("C.y %.8f\n", C.y);
		//printf("C.z %.8f\n", C.z);
		
		//printf("intersectionLines[2][0].x: %.8f\n", intersectionLines[i][0].x);
		//printf("intersectionLines[2][0].y: %.8f\n", intersectionLines[i][0].y);
		//printf("intersectionLines[2][0].z: %.8f\n", intersectionLines[i][0].z);
		
		//printf("intersectionLines[2][1].x: %.8f\n", intersectionLines[i][1].x);
		//printf("intersectionLines[2][1].y: %.8f\n", intersectionLines[i][1].y);
		//printf("intersectionLines[2][1].z: %.8f\n", intersectionLines[i][1].z);
		
		//printf("x: %f\n", 	plane.x);
		//printf("y: %f\n", 	plane.y);
		//printf("z: %f\n\n", plane.z);
	}


	
	//~ int i,j;
	// double *results;
	//~ //pixel_distance *closest = new pixel_distance[7];
	double sum_xx = 0.0, sum_yy = 0.0, sum_xy = 0.0,
		   sum_x = 0.0, sum_y = 0.0, sum_xz = 0.0, sum_yz = 0.0, sum_z = 0.0;
	//~ 
	//~ // allocate
	// matrix_allocate(&aug, 3, 4);
	
	// results = new double[3];
	// memset(results,0,sizeof(double) * 3);
	
	double aug[3][4];
	double results[3];
	//~ 
	//~ //findNeighborhood(h_edge_pxls[pxl], closest);
	//~ 
	//~ //for(int i = 0; i < 6; i++)
	//~ //{
		sum_xx += B.x * B.x;
		sum_yy += B.y * B.y;
		sum_xy += B.x * B.y;
		sum_xz += B.x * B.z;
		sum_yz += B.y * B.z;
		sum_x  += B.x;
		sum_y  += B.y;
		sum_z  += B.z;
	//}
	
		sum_xx += C.x * C.x;
		sum_yy += C.y * C.y;
		sum_xy += C.x * C.y;
		sum_xz += C.x * C.z;
		sum_yz += C.y * C.z;
		sum_x  += C.x;
		sum_y  += C.y;
		sum_z  += C.z;
		//~ 
		//~ sum_xx += intersectionLines.cells[i][0].x * intersectionLines.cells[i][0].x;
		//~ sum_yy += intersectionLines.cells[i][0].y * intersectionLines.cells[i][0].y;
		//~ sum_xy += intersectionLines.cells[i][0].x * intersectionLines.cells[i][0].y;
		//~ sum_xz += intersectionLines.cells[i][0].x * intersectionLines.cells[i][0].z;
		//~ sum_yz += intersectionLines.cells[i][0].y * intersectionLines.cells[i][0].z;
		//~ sum_x  += intersectionLines.cells[i][0].x;
		//~ sum_y  += intersectionLines.cells[i][0].y;
		//~ sum_z  += intersectionLines.cells[i][0].z;
	//~ 
		//~ sum_xx += intersectionLines.cells[i][1].x * intersectionLines.cells[i][1].x;
		//~ sum_yy += intersectionLines.cells[i][1].y * intersectionLines.cells[i][1].y;
		//~ sum_xy += intersectionLines.cells[i][1].x * intersectionLines.cells[i][1].y;
		//~ sum_xz += intersectionLines.cells[i][1].x * intersectionLines.cells[i][1].z;
		//~ sum_yz += intersectionLines.cells[i][1].y * intersectionLines.cells[i][1].z;
		//~ sum_x  += intersectionLines.cells[i][1].x;
		//~ sum_y  += intersectionLines.cells[i][1].y;
		//~ sum_z  += intersectionLines.cells[i][1].z;
		
		sum_xx += midPoint.x * midPoint.x;
		sum_yy += midPoint.y * midPoint.y;
		sum_xy += midPoint.x * midPoint.y;
		sum_xz += midPoint.x * midPoint.z;
		sum_yz += midPoint.y * midPoint.z;
		sum_x  += midPoint.x;
		sum_y  += midPoint.y;
		sum_z  += midPoint.z;
		
				
	//printf("midPoint.x: %.8f\n", midPoint.x );
	//printf("midPoint.y: %.8f\n", midPoint.y );
	//printf("midPoint.z: %.8f\n", midPoint.z );
		
	aug[0][0] = sum_xx;
	aug[0][1] = sum_xy;
	aug[1][0] = sum_xy;
	aug[0][2] = sum_x;
	aug[2][0] = sum_x;
	aug[1][1] = sum_yy;
	aug[1][2] = sum_y;
	aug[2][1] = sum_y;
	aug[2][2] = 3;
	aug[0][3] = sum_xz;
	aug[1][3] = sum_yz;
	aug[2][3] = sum_z;
	
	/*
	printf("before ============================================================\n");
	printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     sum_xz: %f\n", aug[0][0], aug[0][1], aug[0][2], aug[0][3]);
	printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     sum_yz: %f\n", aug[1][0], aug[1][1], aug[1][2], aug[1][3]);
	printf("aug: .... sum_x : %f     sum_y : %f           :%f     sum_z : %f\n", aug[2][0], aug[2][1], aug[2][2], aug[2][3]);
	printf("before ============================================================\n");
	*/

	// run a gaussian solver on the system of equations
	//~ //printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug);
	gauss(aug,3,4,results);
	//~ matrix_print("Augmented (post-gauss)",&aug);
	//~ //printf("==========================================\n");
	
	/*
	printf("after ============================================================\n");
	printf("aug: .... sum_xx: %f     sum_xy: %f     sum_x :%f     sum_xz: %f\n", aug[0][0], aug[0][1], aug[0][2], aug[0][3]);
	printf("aug: .... sum_xy: %f     sum_yy: %f     sum_y :%f     sum_yz: %f\n", aug[1][0], aug[1][1], aug[1][2], aug[1][3]);
	printf("aug: .... sum_x : %f     sum_y : %f           :%f     sum_z : %f\n", aug[2][0], aug[2][1], aug[2][2], aug[2][3]);
	printf("after ============================================================\n");
	*/
	// extract the results
	plane.x = results[0];
	plane.y = results[1];
	plane.z = results[2];
	
	//~ //printf("A: %f\n", plane.x);
	//~ //printf("B: %f\n", plane.y);
	//~ //printf("C: %f\n\n", plane.z);

	// cleanup
	//delete [] results;
}

__device__ bool in_range(int val, int low, int high) {
	return low <= val && val <= high;
}
